
#include "odrive_main.h"

SensorlessEstimator::SensorlessEstimator(Config_t& config) :
        config_(config)
    {};

bool SensorlessEstimator::update() {
    // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer
    // http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
    // In particular, equation 8 (and by extension eqn 4 and 6).

    // The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle
    // is the one computed two cycles ago. To get the correct measurement, it was stored twice:
    // once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory.
    //在与该周期相关的当前测量之前无限应用的V_alpha_beta是两个周期前计算的。
    //为了获得正确的测量结果，它被存储了两次：一次是final_v_alpha/final_v_beta在当前控制报告中存储，一次是V_alpha_beta_memory。

    // Clarke transform
    float I_alpha_beta[2] = {
        -axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC,//-Ib - Ic
        one_by_sqrt3 * (axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC)
        };//1/根号3 * (Ib-Ic)

    // Swap sign of I_beta if motor is reversed 如果电机反转，则交换I_beta标志
    I_alpha_beta[1] *= axis_->motor_.config_.direction;

    // alpha-beta vector operations
    float eta[2];
    for (int i = 0; i <= 1; ++i) {//计算2次 第一次alpha 第二次beta
        // y is the total flux-driving voltage (see paper eqn 4)         y 是总磁通驱动电压（参见论文方程 4）
        // y[0] = 上次的Ialpha - 相电阻 * Ialpha 
        // y[1] = 上次的Ibeta - 相电阻 * Ibeta
        //公式4: y = -R * I_alpha + V_alpha 
        float y = -axis_->motor_.config_.phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory_[i];//V_alpha_beta_memory_是上一次转换svpwm的电压值
        // flux dynamics (prediction) 通量动力学（预测）
        float x_dot = y;
        // integrate prediction to current timestep 
        // 将预测集成到当前时间步长
        // 积分计算
        flux_state_[i] += x_dot * current_meas_period;

        // eta is the estimated permanent magnet flux (see paper eqn 6)
        // ETA是估计的永磁体磁通量（参见论文方程6）
        // eta_Ialpha = 磁通 - 电感 * Ialpha
        // eta_Ibeta = 磁通 - 电感 * Ibeta
        eta[i] = flux_state_[i] - axis_->motor_.config_.phase_inductance * I_alpha_beta[i];
    }

    // Non-linear observer (see paper eqn 8): 
    // 非线性观测器（参见论文方程 8）：
    //config_.pm_flux_linkage--反电动势常数
    float pm_flux_sqr = config_.pm_flux_linkage * config_.pm_flux_linkage;
    float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1];//alpha beta 方向的平方 
    float bandwidth_factor = 1.0f / pm_flux_sqr;
    float eta_factor = 0.5f * (config_.observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr);

    // alpha-beta vector operations
    for (int i = 0; i <= 1; ++i) {
        // add observer action to flux estimate dynamics
        // 将观察者操作添加到通量估计动态
        float x_dot = eta_factor * eta[i];
        // convert action to discrete-time
        flux_state_[i] += x_dot * current_meas_period;
        // update new eta
        eta[i] = flux_state_[i] - axis_->motor_.config_.phase_inductance * I_alpha_beta[i];
    }

    // Flux state estimation done, store V_alpha_beta for next timestep
    // 完成磁通状态估计，存储V_alpha_beta以备下一个时间步长使用
    V_alpha_beta_memory_[0] = axis_->motor_.current_control_.final_v_alpha;//svpwm中转换的Ualpha Ubeta
    V_alpha_beta_memory_[1] = axis_->motor_.current_control_.final_v_beta * axis_->motor_.config_.direction;

    // ----------------------PLL---------------------------------
    // ----------------------PLL---------------------------------
    // TODO: the PLL part has some code duplication with the encoder PLL
    // Pll gains as a function of bandwidth
    float pll_kp = 2.0f * config_.pll_bandwidth;
    // Critically damped 临界阻尼
    float pll_ki = 0.25f * (pll_kp * pll_kp);
    // Check that we don't get problems with discrete time approximation
    // 检查我们是否没有遇到离散时间近似的问题
    if (!(current_meas_period * pll_kp < 1.0f)) {
        error_ |= ERROR_UNSTABLE_GAIN;
        vel_estimate_valid_ = false;
        return false;
    }

    // predict PLL phase with velocity
    // 用速度预测 PLL 相位
    pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * vel_estimate_erad_);
    // update PLL phase with observer permanent magnet phase
    // 使用观察者永磁相位更新 PLL 相位
    phase_ = fast_atan2(eta[1], eta[0]);//得到相位  用于电流环使用

    float delta_phase = wrap_pm_pi(phase_ - pll_pos_);//相位误差
    pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_kp * delta_phase);
    // update PLL velocity
    // 更新 PLL 速度
    vel_estimate_erad_ += current_meas_period * pll_ki * delta_phase;
    // convert to mechanical turns/s for controller usage.
    // 转换为机械转/秒供控制器使用。
    vel_estimate_ = vel_estimate_erad_ / (std::max((float)axis_->motor_.config_.pole_pairs, 1.0f) * 2.0f * M_PI);

    vel_estimate_valid_ = true;
    return true;
};
